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Abstract  We  propose  a  novel  enhancement  to  the  task  of  collision-testing  a  set  of 
local  paths.  Our  approach  circumvents  expensive  collision-tests,  yet  it  declares  a 
continuum  of  paths  collision-free  by  exploiting  both  the  structure  of  paths  and  the 
outcome  of  previous  tests.  We  define  a  homotopy-like  equivalence  relation  among 
local  paths  and  provide  algorithms  to  (1)  classify  paths  based  on  equivalence,  and 
(2)  implicitly  collision-test  up  to  90%  of  them.  We  then  prove  both  correctness  and 
completeness  of  these  algorithms  before  providing  experimental  results  showing  a 
performance  increase  up  to  300%. 


1  Introduction 

Planning  bounded-curvature  paths  for  mobile  robots  is  an  NP-hard  problem  [22]. 
Many  nonholonomic  mobile  robots  thus  rely  on  hierarchical  planning  architec¬ 
tures  [1,  13,  19],  which  split  responsibility  between  at  least  two  layers  (Fig.  1):  a 
slow  global  planner  and  fast  local  planner.  We  focus  here  on  the  local  planner  (Alg.  1 
and  Alg.  2),  which  iterates  in  a  tight  loop:  searching  through  a  set  of  paths  and  se¬ 
lecting  the  best  path  for  execution.  During  each  loop,  the  planner  tests  many  paths 
before  making  an  informed  decision.  The  bottleneck  in  path  testing  is  collision¬ 
testing  [24] .  In  this  paper,  we  introduce  a  novel  approach  that  delivers  a  significant 
increase  in  path  set  collision-testing  performance  by  exploiting  the  fundamental  ge¬ 
ometric  structure  of  paths. 

We  introduce  an  equivalence  relation  intuitively  resembling  the  topological  no¬ 
tion  of  homotopy.  Two  paths  are  path  homotopic  if  a  continuous,  collision-free  de¬ 
formation  with  fixed  start  and  end  points  exists  between  them  [20].  Like  any  path 
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Fig.  1  An  example  hier¬ 
archical  planning  scenario. 
The  local  planner’s  path  set 
expands  from  the  robot,  at 
center,  and  feeds  commands 
to  the  robot  based  on  the  best 
path  that  avoids  obstacles 
(black  squares).  The  chosen 
local  path  (green)  and  global 
path  (red)  combine  to  form  a 
proposed  path  to  the  goal. 


.  "l 


equivalence  relation,  homotopy  partitions  paths  into  equivalence  classes.  Different 
homotopy  classes  make  fundamentally  different  choices  about  their  route  amongst 
obstacles.  However,  two  mobile  robot  concepts  translate  poorly  into  homotopy  the¬ 
ory:  limited  sensing  and  constrained  action. 

The  robot  may  lack  a  complete  workspace  map,  which  must  instead  be  con¬ 
structed  from  sensor  data.  Since  robot  perception  is  limited  by  range  and  occlusion, 
a  robot’s  understanding  of  obstacles  blocking  its  movement  evolves  with  its  vantage 
point.  A  variety  of  sensor-based  planning  algorithms  have  been  developed  to  handle 
such  partial  information.  Obstacle  avoidance  methods,  such  as  potential  fields  [12], 
are  purely  reactive.  The  bug  algorithm  [18],  which  generates  a  path  to  the  goal  us¬ 
ing  only  a  contact  sensor,  is  complete  in  2D.  Choset  and  Burdick  [5]  present  the 
hierarchical  generalized  Voronoi  graph,  a  roadmap  with  global  line-of-sight  acces¬ 
sibility  that  achieves  completeness  in  higher  dimensions  using  range  readings  of  the 
environment. 

If  a  robot  is  tasked  to  perform  long-range  navigation,  then  it  must  plan  a  path 
through  unsensed  regions.  A  low-fidelity  global  planner  generates  this  path  because 
we  prefer  to  avoid  significant  investment  in  this  plan,  which  will  likely  be  invalidated 
later.  Path  homotopy,  in  the  strictest  sense,  requires  global  knowledge  of  obstacles 
because  homotopy  equivalent  paths  must  connect  fixed  start  and  goal  points. 

Relaxing  the  endpoint  requirement  avoids  reasoning  about  the  existence  of  far¬ 
away,  unsensed  obstacles.  Naively  relaxing  a  fixed  endpoint,  our  paths  might  be 
permitted  to  freely  deform  around  obstacles,  making  all  paths  equivalent.  To  re¬ 
store  meaningful  equivalence  classes,  we  propose  an  alternate  constraint  based  on 
path  shape.  This  is  in  keeping  with  the  nonholonomic  constraints  that  limit  mo¬ 
bile  robots’  action.  Laumond  [15]  first  highlighted  the  importance  of  nonholonomic 
constraints  and  showed  that  feasible  paths  exist  for  a  mobile  robot  with  such  con¬ 
straints.  Barraquand  and  Latombe  [2]  created  a  grid-based  planner  that  innately 
captures  these  constraints.  LaValle  and  Kuffner  [17]  proposed  the  first  planner  to 
incorporate  both  kinodynamic  constraints  and  random  sampling.  In  contrast  to  non¬ 
holonomic  constraints,  true  homotopy  forbids  restrictions  on  path  shape;  two  paths 
are  equivalent  if  any  path  deformation — however  baroque — exists  between  them. 
By  restricting  our  paths  to  bounded  curvature,  we  represent  only  feasible  motions 
while  limiting  paths’  ability  to  deform  around  obstacles.  The  resulting  set  of  path 
equivalence  classes  is  of  immediate  importance  to  the  planner  (Fig.  2).  The  number 


An  Equivalence  Relation  for  Local  Path  Sets 


3 


Fig.  2  left:  Paths  from  a  few  distinct  homotopy  classes  between  the  robot  and  the  goal.  The  dis¬ 
tinctions  between  some  classes  require  information  that  the  robot  has  not  yet  sensed  (the  dark  area 
is  out  of  range  or  occluded),  middle:  With  paths  restricted  to  the  sensed  area,  they  may  freely  de¬ 
form  around  visible  obstacles,  right:  After  restricting  path  shape  to  conform  to  motion  constraints, 
we  get  a  handful  of  equivalence  classes  that  are  immediately  applicable  to  the  robot. 


of  choices  represented  by  these  local  equivalence  classes  relates  to  Farber’s  topo¬ 
logical  complexity  of  motion  planning  [6]. 

Equivalence  classes  have  been  employed  in  various  planners.  In  task  planning,  re¬ 
cent  work  has  shown  that  equivalence  classes  of  actions  can  be  used  to  eliminate  re¬ 
dundant  search  [7].  In  motion  planning,  path  equivalence  often  employs  homotopy. 
A  recent  paper  by  Bhattacharya,  Kumar,  and  Likhachev  [3]  provides  a  technique 
based  on  complex  analysis  for  detecting  homotopic  equivalence  among  paths  in  2D. 
Two  papers  employing  equivalence  classes  to  build  probabilistic  roadmaps  [11]  are 
by  Schmitzberger,  et  al.  [25]  and  Jaillet  and  Simeon  [10].  The  latter  paper  departs 
from  true  homotopy  by  proposing  the  visibility  deformation,  a  simplified  alternative 
to  homotopic  equivalence  based  on  line-of-sight  visibility  between  paths. 

Our  key  insight  is  that  local  path  equivalence  is  an  expressive  and  powerful  tool 
that  reveals  shared  outcomes  in  collision-testing.  Specihcally,  two  equivalent  neigh¬ 
boring  paths  cover  some  common  ground  in  the  workspace,  and  between  them  lies 
a  continuum  of  covered  paths.  We  develop  the  mathematical  foundations  to  detect 
equivalence  relations  among  all  local  paths  based  on  a  hnite  precomputed  path  set. 
We  then  utilize  these  tools  to  devise  efficient  algorithms  for  detecting  equivalence 
and  implicitly  collision-testing  local  paths. 

The  remainder  of  the  paper  is  organized  as  follows.  We  provide  an  implementa¬ 
tion  of  the  basic  algorithm  in  Section  2  and  present  the  fast  collision-testing  tech¬ 
nique.  Section  3  then  explores  the  theoretical  foundations  of  our  path  equivalence 
relation.  Section  4  provides  some  experimental  results. 


2  Algorithms 

In  this  section,  we  present  three  algorithms:  path  set  generation,  path  classification, 
and  implicit  path  collision-testing.  All  of  the  algorithms  presented  here  run  in  poly¬ 
nomial  time.  Throughout  this  paper,  we  use  lowercase  p  to  refer  to  a  path  in  the 
workspace,  while  T  is  a  set  of  paths  (each  one  a  point  in  path  space). 
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Algorithm  1  Test_AlLPaths(w,  CP) 

Input:  w  -  a  costmap  object;  tP  -  a  fixed  set  of  paths 

Output:  tP  free,  the  set  of  paths  that  passed  collision  test 

1 

T/rce  ^  0 

2 

while  time  not  expired  and  untested  paths  remain  do 

H  test  paths  for  0. 1  seconds 

3 

p  ^  Get  ext _PathiV) 

4 

collision  ^  w.TestJ'ath(p) 

//  collision  is  boolean 

5 

if  not  collision  then 

6 

‘P free  ^  it* free  G  {p} 

//  non-colliding  path  set 

7 

return  iPfree 

Algorithm  2  LocalJPlanner^lgorithm(w,  x,  h,  CP) 

Input:  w  -  a  costmap  object;  x  -  initial  state;  h-a  heuristic  function  for  selecting  a  path  to  execute; 
IP  -  a  fixed  set  of  paths 

Output:  Moves  the  robot  to  the  goal  if  possible 
1 :  while  not  at  goal  and  time  not  expired  do 
2:  V free  Test-All-Paths(w,x,'P) 

3:  j  ^  h.Best_Path(x,  IP  free) 

4:  Execute-Path.OnJiobot(j) 

5:  X*—  Predict  _Next  _State{x,  j) 


Definition  1.  Path  space  is  a  metric  space  (CP,/4)  in  which  the  distance  between  a 
pair  of  paths  in  CP  is  defined  by  metric  p.  Paths  can  vary  in  shape  and  length.  □ 


2.1  Path  Set  Generation 

We  use  the  greedy  path  set  construction  technique  of  Green  and  Kelly  [8],  outlined 
in  Alg.  3.  The  algorithm  iteratively  builds  a  path  set  CPa:  by  drawing  paths  from  a 
densely-sampled  source  path  set,  X.  During  step  i,  it  selects  the  path  p  G  X  that 
minimizes  the  dispersion  of  CP,  =  CP,_i  U  {p}.  Borrowing  from  Niederreiter  [21]: 

Definition  2.  Given  a  bounded  metric  space  (X,/4)  and  point  set  CP  =  {xi,. . .  ,xn}  G 
X,  the  dispersion  of  CP  in  X  is  defined  by 

5(CP,  X)  =  supmin/4(x,p).  □  (1) 

The  dispersion  of  CP  in  X  equals  the  radius  of  the  biggest  open  ball  in  X  containing 
no  points  in  CP.  By  minimizing  dispersion,  we  ensure  that  there  are  no  large  voids 
in  path  space.  Thus,  dispersion  reveals  the  quality  of  CP  as  an  “approximation”  of 
X  because  it  guarantees  that  for  any  x  G  X,  there  is  some  point  p  S  CP  such  that 
ld{,x,p)  <  5(CP,X). 

The  Green-Kelly  algorithm  generates  a  sequence  of  path  sets  CP,  ,  for  /  G  { 1 , . . . ,  A}, 
that  has  monotonically  decreasing  dispersion.  Alg.  1  searches  paths  in  this  order  at 
runtime,  thus  permitting  early  termination  while  retaining  near-optimal  results.  Note 
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that  while  the  source  set  X  is  of  finite  size — providing  a  lower  bound  on  dispersion 
at  runtime — it  can  be  chosen  with  arbitrarily  low  dispersion  a  priori. 


Algorithm  3  Green_Kelly(X,  N) 

Input:  X  -  a  densely-sampled,  low-dispersion  path  set;  W  <  |X|  -  the  target  path  set  size 
Output:  path  sequence  of  size  N 

1:  a>o^0 

2:  n^O 

3:  while  n<  N  do 

4:  n  ^  n+l 

5:  p  ^  argmin  U  {x},  X) 

xeX 

6:  ^  Pb-I  U  {p} 

7:  return  CPjv 


2.2  Path  Classification 

We  next  present  Alg.  4,  which  classifies  collision-free  members  of  a  path  set.  The 
Hausdorff  metric  is  central  to  the  algorithm.  Intuitively,  this  metric  returns  the  great¬ 
est  amount  of  separation  between  two  paths  in  the  workspace.  From  Munkres  [20]: 

I^H{pi,Pj)  =  inf{e :  Pi  C  {pj)e  and  pj  C  {pi)e},  (2) 

where  {p)r  denotes  dilation  of  p  by  r:  {t  G  .  \\tp-t\\L2  <  r  for  some  tp  G  p}. 
Note  that  pn  satisfies  all  properties  of  a  metric  [9].  For  our  fixed  path  set  generated 
by  Green-Kelly,  we  precomputed  each  pairwise  path  metric  value  of  (2)  and  stored 
them  in  a  lookup  table  for  rapid  online  access. 

Alg.  4  performs  path  classification  on  a  set  of  paths  that  have  already  tested 
collision-free  at  runtime.  We  form  a  graph  G  =  {V,E)  in  which  node  v,  G  V  corre¬ 
sponds  to  path  Pi.  Edge  eij  G  E  joins  nodes  v,  and  vj  when  this  relation  holds: 


I^H{pi,Pj)  <  d,  (3) 

where  d  is  the  diameter  of  the  robot.  Taking  the  transitive  closure  of  this  relation, 
two  paths  Pa  and  ph  are  equivalent  if  nodes  Va  and  v*  are  in  the  same  connected 
component  of  G  (Fig.  3). 

In  effect,  this  algorithm  constructs  a  probabilistic  roadmap  (PRM)  in  the  path 
space  instead  of  the  conventional  configuration  space.  A  query  into  this  PRM  tells 
whether  two  paths  are  equivalent.  As  with  any  PRM,  a  query  is  performed  by  adding 
two  new  graph  nodes  Vj  and  Vg  corresponding  to  the  two  paths.  We  attempt  to  join 
these  nodes  to  other  nodes  in  the  graph  based  on  (3).  The  existence  of  a  path  con¬ 
necting  Vi  to  Vg  indicates  path  equivalence. 
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Fig.  3  A  simple  path  set, 
in  which  obstacles  (black) 
eliminate  colliding  paths. 

The  collision-free  path  set 
has  three  equivalence  classes 
(red,  green,  and  blue).  In  the 
corresponding  graph  repre¬ 
sentation,  at  right,  adjacent 
nodes  represent  proximal 
paths.  Connected  components 
indicate  equivalence  classes 
of  paths. 


Algorithm  4  Equivalence Classes(CP/ree,  d) 

Input:  IP  free  “  a  set  of  safe,  appropriate  paths;  d  - 

the  diameter  of  the  robot 

Output:  Z)  -  a  partition  of  IP  free  into  equivalence  classes  (a  set  of  path  sets) 

1 

LetG=  (k,£)  ^  (0,0) 

2 

£)^0 

3 

for  all  Pi  6  IP  free  do 

//  This  loop  discovers  adjacency 

4 

V.add{pi) 

//  Add  a  graph  node  corresponding  to  path  p,- 

5 

for  all  pj  6  F  \  {p,}  do 

6 

if  <4  then 

7 

E.add{i,j)  //  Connect  nodes  i  and  j  with  an  unweighted  edge 

8 

S  ^  IP  free 

9 

while  S  /  0  do 

//  This  loop  finds  the  connected  components 

10 

e^0 

11 

p  <—  a  member  of  S 

12 

C  -  {p} 

//  List  of  nodes  to  be  expanded  in  this  class 

13 

while  Si  ftHAo 

14 

p  ^  a  member  of  Si 

15 

eu{p} 

//  Commit  p  to  class 

16 

S^S\{p} 

17 

C  ^  (C  U  V.neighbors{p))  n  S 

18 

D^DU{e} 

19 

return  D 

2.3  Implicit  Path  Safety  Test 

There  is  an  incessant  need  in  motion  planning  to  accelerate  collision-testing,  which 
may  take  99%  of  total  CPU  time  [24].  During  collision-testing,  the  planner  must 
verify  that  a  given  swath  is  free  of  obstacles. 

Definition  3.  A  swath  is  the  workspace  area  of  ground  or  volume  of  space  swept 
out  as  the  robot  traverses  a  path.  □ 

Definition  4.  We  say  a  path  is  safe  if  its  swath  contains  no  obstacles.  □ 

In  testing  many  swaths  of  a  robot  passing  through  space,  most  planners  ef¬ 
fectively  test  the  free  workspace  many  times  by  testing  overlapping  swaths.  We 
may  test  a  path  implicitly  at  significant  computational  savings  by  recalling  recent 
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collision-testing  outcomes.  We  formalize  the  idea  in  Alg.  5,  which  is  designed  to  be 
invoked  from  Alg.  1,  line  4  in  lieu  of  the  standard  path  test  routine. 

The  implicit  collision-test  condition  resembles  the  neighbor  condition  (3)  used 
by  Alg.  4,  but  it  has  an  additional  “IsJBetween”  check,  which  indicates  that  the 
swath  of  the  path  under  test  is  covered  by  two  collision-free  neighboring  swaths. 
The  betweenness  trait  can  be  precomputed  and  stored  in  a  lookup  table.  Given  a  set 
of  safe  paths,  we  can  quickly  discover  whether  any  pair  covers  the  path  under  test. 
Experimental  results  show  that  this  algorithm  allows  us  to  test  up  to  90%  of  paths 
implicitly,  thus  increasing  the  path  evaluation  rate  by  up  to  300%  in  experiments. 


Algorithm  5  Test_Path  Jmplicit(p,  w,  §,  d) 

Input:  p  is  a  path  to  be  tested 

Input:  w  is  a  costmap  object  //  used  as  a  backup  when  path  cannot  be  implicitly  tested 

Input:  S  is  the  set  of  safe  paths  found  so  far 
Input:  d  is  the  diameter  of  the  robot 
1:  for  all  Pi,py  6  S  such  that  llH{Pi,Pj)  <  d  do 

2:  if  p.IsJietweenipi,  p  j)  then  //  p’s  swath  has  been  tested  previously 

3:  Sf  ^  p.Get_End_Point() 

4:  collision  ^  w.TestJ’oint{sf)  //  endpoint  may  not  be  covered  by  swaths 

5:  return  collision 

6:  return  w.TestJ’ath{p)  // Fall  back  to  explicit  path  test 


3  Foundations 

In  this  section,  we  establish  the  foundations  of  an  equivalence  relation  on  path 
space  based  on  continuous  deformations  between  paths.  We  then  provide  correct¬ 
ness  proofs  for  our  algorithms  for  classification  and  implicit  collision-testing. 

We  assume  a  kinematic  description  of  paths.  All  paths  are  parametrized  by 
a  shared  initial  pose,  shared  fixed  length,  and  individual  curvature  function.  Let 
Ki{s)  describe  the  curvature  control  of  path  i  as  a  function  of  arc  length,  with 
maxo<s<s^  I  Ki{s)  I  <  iCmctt-  Typical  expressions  for  Ki  include  polynomials,  piecewise 
constant  functions,  and  piecewise  linear  functions.  The  robot  motion  produced  by 
control  i  is  a  feasible  path  given  by 


'Oi' 

Ki 

Xi 

= 

COS  9i 

Ji. 

sin  9i 

Definition  5.  A  feasible  path  has  bounded  curvature  (implying  C*  continuity)  and 
fixed  length.  The  set  T(sp,  Kmax)  contains  all  feasible  paths  of  length  Sf  and  curva¬ 
ture  |k*(5')|  ^  ^ 
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Fig.  4  At  top:  several  ex¬ 
ample  paths  combining  dif¬ 
ferent  values  of  v  and  w. 
Each  path  pair  obeys  (3). 
The  value  of  v  affects  the 
“curviness”  allowed  in  paths, 
while  w  affects  their  length. 
At  bottom:  this  plot,  gen¬ 
erated  numerically,  approx¬ 
imates  the  set  of  appropri¬ 
ate  choices  for  v  and  w. 

The  gray  region  at  top  right 
must  be  avoided,  as  we  show 
in  Lemma  2.  Such  choices 
would  permit  an  obstacle  to 
occur  between  two  safe  paths 
that  obey  (3).  A  path  whose 
values  fall  in  the  white  region 
is  called  an  appropriate  path. 


3.1  Properties  of  Paths 

In  this  section,  we  establish  a  small  set  of  conditions  under  which  we  can  quickly  de¬ 
termine  that  two  paths  are  equivalent.  We  constrain  path  shape  through  two  dimen¬ 
sionless  ratios  relating  three  physical  parameters.  We  may  then  detect  equivalence 
through  a  simple  test  on  pairs  of  paths  using  the  Hausdorff  metric. 

These  constraints  ensure  a  continuous  deformation  between  neighboring  paths 
while  permitting  a  range  of  useful  actions.  Many  important  classes  of  action  sets 
obey  these  general  constraints,  including  the  line  segments  common  in  RRT  [17]  and 
PRM  planners,  as  well  as  constant  curvature  arcs.  Fig.  1  illustrates  a  more  expressive 
action  set  [13]  that  adheres  to  our  constraints. 

The  three  physical  parameters  are:  d,  the  diameter  of  the  robot;  Sf,  the  length  of 
each  path;  and  the  minimum  radius  of  curvature  allowed  for  any  path.  Note 
that  1  / rmin  =  Kmax.  the  upper  bound  on  curvature.  For  non-circular  robots,  d  reflects 
the  minimal  cross-section  of  the  robot’s  swath  sweeping  along  a  path.  We  express 
relationships  among  the  three  physical  quantities  by  two  dimensionless  parameters: 

d  Sf 

v= -  >v=- - . 

We  only  compare  paths  with  like  values  of  v  and  w.  Fig.  4(top)  provides  some  intu¬ 
ition  on  the  effect  of  these  parameters  on  path  shape.  Due  to  the  geometry  of  paths, 
only  certain  choices  of  v  and  w  are  appropriate. 
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Definition  6.  An  appropriate  path  is  a  feasible  path  conforming  to  appropriate  val¬ 
ues  of  V  and  w  from  the  proof  of  Lemma  2.  Fig.  4  previews  the  permissible  values. 

When  the  condition  in  (3)  is  met,  the  two  paths’  swaths  overlap,  resulting  in  a 
continuum  of  coverage  between  the  paths.  This  coverage,  in  turn,  ensures  the  exis¬ 
tence  of  a  continuous  deformation,  as  we  show  in  Theorem  1,  but  first  we  formally 
define  a  continuous  deformation  between  paths. 

Definition  7.  A  continuous  deformation  between  two  safe,  feasible  paths  pi  and  pj 
in  3^(5/-,  Kmax)  is  a  continuous  function  /:  [0, 1]  ^  >  ^max)^  with  s^  slightly  less 

than  Sf  and  slightly  more  than  Kmax-  /(O)  is  the  initial  interval  of  p,,  and  /(I) 
is  the  initial  interval  of  pj,  both  of  length  s^ .  We  write  p,  ~  pj  to  indicate  that 
a  continuous  deformation  exists  between  paths  p,  and  pj,  and  they  are  therefore 
equivalent.  □ 

The  length  s^  depends  on  v  and  w,  but  for  typical  values,  s^  is  fully  95-98%  of 
Sf.  For  many  applications,  this  is  sufficient,  but  an  application  can  quickly  test  the 
remaining  path  length  if  necessary.  Nearly  all  paths  /(c)  are  bounded  by  curvature 
t^max,  but  it  will  turn  out  that  in  certain  geometric  circumstances,  the  maximum 
curvature  through  a  continuous  deformation  is  up  to  Kf^^  =  ^Kmax- 

Definition  8.  Two  safe,  feasible  paths  that  define  a  continuous  deformation  are 
called  guard  paths  because  they  protect  the  intermediate  paths.  □ 

In  the  presence  of  obstacles,  it  is  not  trivial  to  determine  whether  a  continuous 
deformation  is  safe,  thus  maintaining  equivalency.  Rather  than  trying  to  find  a  defor¬ 
mation  between  arbitrary  paths,  we  propose  a  particular  condition  under  which  we 
show  that  a  bounded-curvature,  fixed-length,  continuous  path  deformation  exists, 

ilH{pi,P2)<d  p\^  P2.  (5) 

This  statement,  which  we  prove  in  the  next  section,  is  the  basis  for  Alg.  4  and  Alg.  5. 
The  overlapping  swaths  of  appropriate  paths  pi  and  p2  cover  a  continuum  of  inter¬ 
mediate  swaths  between  the  two  paths.  Eqn.  (5)  is  a  proper  equivalence  relation 
because  it  possesses  each  of  three  properties: 

•  reflexivity.  /r//(p,p)  =  0;  p  is  trivially  deformable  to  itself. 

•  symmetry.  The  Hausdorff  metric  is  symmetric. 

•  transitivity.  Given  fl//(pi,p2)  <  and  Ph{P2,P3)  <  d,  a  continuous  deforma¬ 
tion  from  Pi  to  p3  passes  through  p2. 


3.2  Equivalence  Relation 

Having  presented  the  set  of  conditions  under  which  (5)  holds,  we  now  prove  that 
they  are  sufficient  to  ensure  the  existence  of  a  continuous  deformation  between  two 


10 


Ross  A.  Knepper,  Siddhartha  S.  Srinivasa,  and  Matthew  T.  Mason 


Fig.  5  Paths  Pi,Pj,  and  p^ 
form  boundary  B.  Its  interior, 
/,  contains  all  paths  in  the 
continuous  deformation  from 
Pi  to  Pj. 


neighboring  paths.  Our  approach  to  the  proof  will  be  to  hrst  describe  a  feasible 
continuous  deformation,  then  show  that  paths  along  this  deformation  are  safe. 

Given  appropriate  guard  paths  p,  and  pj  with  common  origin,  let  pg  be  the  short¬ 
est  curve  in  the  workspace  connecting  their  endpoints  without  crossing  either  path 
{pe  may  pass  through  obstacles).  The  closed  path  B  =  p;  U  pj  U  pe  creates  one  or 
more  closed  loops  (the  paths  may  cross  each  other).  By  the  Jordan  curve  theorem, 
each  loop  partitions  into  two  sets,  only  one  of  which  is  compact.  Let  /,  the  inte¬ 
rior,  be  the  union  of  these  compact  regions  with  B,  as  in  Fig.  5. 

Definition  9.  A  path  pc  is  between  paths  p,  and  pj  if  pc  Cl.  □ 

Lemma  1.  Given  appropriate  paths  Pi,Pj  C  T(s/,  Kmax)  with  p.H{pi,Pj)  <d,  a  path 
sequence  exists  in  the  form  of  a  feasible  continuous  deformation  between  pi  and  pj. 

Proof  We  provide  the  form  of  a  continuous  deformation  from  p,-  to  pj  such  that 
each  intermediate  path  is  between  them.  With  t  a  workspace  point  and  p  a  path,  let 


7(f,p)  =  inf{£;  f  e  (p)e}  (6) 

r  [0, 1]  if  7(f,p;)  =  7(f,p;)  =  0 

~l{y(J)+;(U)}  otherwise, 

where  g{t)  is  a  set-valued  function  to  accommodate  intersecting  paths.  Each  level 
set  g{t)  =  c  for  c  G  [0,1]  dehnes  a  weighted  generalized  Voronoi  diagram  (GVD) 
forming  a  path  as  in  Fig.  6.  We  give  the  form  of  a  continuous  deformation  using 
level  sets  g^t(c);  each  path  is  parametrized  starting  at  the  origin  and  extending  for 
a  length  sf  in  the  direction  of  pg.  Let  us  now  pin  down  the  value  of  s^.  Every  point 
ti  on  Pi  forms  a  line  segment  projecting  it  to  its  nearest  neighbor  tj  on  pj  (and  vice 
versa).  Their  collective  area  is  shown  in  Fig.  7.  Eqn.  (3)  bounds  each  segment’s 
length  at  d.  s^  is  the  greatest  value  such  that  no  intermediate  path  of  length  s^ 
departs  from  the  region  covered  by  these  projections. 

For  general  shapes  in  the  GVD  forms  a  set  of  curves  meeting  at  branching 
points  [23].  In  this  case,  no  GVD  cusps  or  branching  points  occur  in  any  interme¬ 
diate  path.  Since  d  <  rmin,  no  center  of  curvature  along  either  guard  path  can  fall  in 
I  [4].  Therefore,  each  level  set  defines  a  path  through  the  origin. 

Each  path’s  curvature  function  is  piecewise  continuous  and  everywhere  bounded. 
A  small  neighborhood  of  either  guard  path  approximates  constant  curvature.  A  GVD 
curve  generated  by  two  constant-curvature  sets  forms  a  conic  section  [27].  Table  1 
reflects  that  the  curvature  of  pc  is  everywhere  bounded  with  the  maximum  possible 
curvature  being  bounded  by  ^Kmax-  For  the  full  proofs,  see  [14].  □ 
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Fig.  6  In  a  continuous  de¬ 
formation  between  paths  pi 
and  py,  as  defined  by  the  level 
sets  of  (7),  each  path  takes 
the  form  of  a  weighted  GVD. 
Upper  bounds  on  curvature 
vary  along  the  deformation, 
with  the  maximum  bound  of 
j  Kmux  occurring  at  the  medial 
axis  of  the  two  paths. 


Fig.  7  Hausdorff  coverage  (overlapping  red  and  blue  shapes  in  center)  is  a  conservative  approxi¬ 
mation  of  swath  coverage  (gray).  The  Hausdorff  distance  between  paths  p;  and  pj  is  equal  to  the 
maximum-length  projection  from  any  point  on  either  path  to  the  closest  point  on  the  opposite  path. 
Each  projection  implies  a  line  segment.  The  set  of  projections  from  the  top  line  (blue)  and  bottom 
line  (red)  each  cover  a  solid  region  between  the  paths.  These  areas,  in  turn,  cover  a  slightly  shorter 
intermediate  path  pc,  in  white,  with  its  swath  in  cyan.  This  path’s  length,  is  as  great  as  possible 
while  remaining  safe,  with  its  swath  inside  the  gray  area. 

Table  1  Conic  sections  form  the  weighted  Voronoi  diagram.  Ki  and  K’2  represent  the  curvatures  of 
the  two  guard  paths,  with  iCi  the  lesser  magnitude.  Let  K„,  =  max(  |  (Ci  | ,  |  K2 1 ) .  For  details,  see  [  14] . 


Type 

OccuiTence 

Curvature  bounds  of  intermediate  paths 

line 

Kl  =  -K'2 

|k|  <  K„ 

parabola 

Ki  =  0,  K2  7^  0 

K  <  K„ 

hyperbola 

Kl  K2  <  0,  Kl  f  -  K2 

|k  <  K„ 

ellipse 

Kl  K2  >  0 

|k|  <  ^K„, 

Lemma  2.  Given  safe,  appropriate  guard  paths  Pi,Pj  C  3^{sf,K„,ax)  separated  by 
PH{pi,Pj)  <  d,  any  path  pc  C  between  them  is  safe. 

Proof  We  prove  this  lemma  by  contradiction.  Assume  an  obstacle  lies  between  pt 
and  Pj.  We  show  that  this  assumption  imposes  lower  bounds  on  v  and  w.  We  then 
conclude  that  for  lesser  values  of  v  and  w,  no  such  obstacle  can  exist. 

Let  sl{p,d)  =  {t  £  =  nn{t,p) :  Tft  ±  p  and  ||f  —  fp||i2  ^5}  dehne  a  con¬ 

servative  approximation  of  a  swath,  obtained  by  sweeping  a  line  segment  of  length 
d  with  its  center  along  the  path,  tft  is  the  line  segment  joining  tp  to  t  and  nn(t,p) 
is  the  nearest  neighbor  of  point  t  on  path  p.  The  two  swaths  form  a  safe  region, 
U  =  sl{pi,d)\Jsl{pj,d). 

Suppose  that  U  contains  a  hole,  denoted  by  the  set  h,  which  might  contain  an 
obstacle.  Now,  consider  the  shape  of  the  paths  that  could  produce  such  a  hole.  Be- 
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Fig.  8  (a)  With  bounded  curvature,  there  is  a  lower  bound  on  path  lengths  that  permit  a  hole,  h, 
while  satisfying  (3).  Shorter  path  lengths  ensure  the  existence  of  a  safe  continuous  deformation 
between  paths,  (b)  We  compute  the  maximal  path  length  that  prevents  a  hole  using  Vendittelli’s 
solution  to  the  shortest  path  for  a  Dubins  car.  Starting  from  the  dot  marked  s,  we  find  the  shortest 
path  intersecting  the  circle  D.  The  interval  p?  illustrates  path  lengths  permitting  a  hole  to  exist. 


ginning  with  equal  position  and  heading,  they  must  diverge  widely  enough  to  sepa¬ 
rate  by  more  than  d.  To  close  the  loop  in  U,  the  paths  must  then  bend  back  towards 
each  other.  Since  the  paths  separate  by  more  than  d,  there  exist  two  open  intervals 
pf  C  Pi  and  p*  C  pj  surrounding  the  hole  on  each  path  such  that  (at  this  point) 
pf  {Pj)d  and  py  ^  {Pi)d-  To  satisfy  (3),  there  must  exist  later  intervals  Pi  C  Pi 
such  that  p^  C  (p;)d  and  likewise  p®  C  pj  such  that  p*  C  {p‘j)d,  as  in  Fig.  8a. 

How  long  must  a  path  be  to  satisfy  this  condition?  Consider  the  minimum  length 
solution  to  this  problem  under  bounded  curvature.  For  each  point  t  G  p*,  the  interval 
pf  must  intersect  the  open  disc  D  =  int((f)(/),  as  in  Fig.  8b.  Since  p*  grows  with  the 
width  of  h,  and  pf  must  intersect  all  of  these  open  neighborhoods,  the  path  becomes 
longer  with  larger  holes.  We  will  therefore  consider  the  minimal  small-hole  case. 

Vendittelli  [26]  solves  the  shortest  path  problem  for  a  Dubins  car  to  reach  a  line 
segment.  We  may  approximate  the  circular  boundary  of  D  by  a  set  of  arbitrarily 
small  line  segments.  One  may  show  from  this  work  that  given  the  position  and  slope 
of  points  along  any  such  circle,  the  shortest  path  to  reach  its  boundary  (and  thus  its 
interior)  is  a  constant-curvature  arc  of  radius  In  the  limit,  as  v  approaches  one 
and  the  size  of  h  approaches  zero,  the  length  of  arc  needed  to  satisfy  (3)  approaches 
%I2  from  above,  resulting  in  the  condition  that  w  >  0.48.  Thus,  for  w  <  0.48  and 
V  G  [0, 1),  Pc  is  safe.  For  smaller  values  of  v,  D  shrinks  relative  to  rm/n,  requiring 
longer  paths  to  reach,  thus  allowing  larger  values  of  w  as  shown  in  the  plot  in  Fig.  4. 

We  have  shown  that  there  exist  appropriate  choices  for  v  and  w  such  that  (3) 
implies  that  U  contains  no  holes.  Since  U  contains  the  origin,  any  path  Pc  G  I  ema¬ 
nating  from  the  origin  passes  through  U  and  is  safe.  □ 


Theorem  1.  Given  safe,  appropriate  guard  paths  Pi,Pj  G  3^{sf,K„ax),  ond  given 
PuiPiiPj)  d,  a  safe  continuous  deformation  exists  between  pi  and pj. 
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Proof.  Lemma  1  shows  that  (7)  gives  a  continuous  deformation  between  paths  p, 
and  pj  such  that  each  intermediate  path  C  I  is  feasible.  Lemma  2  shows  that  any 
such  path  is  safe.  Therefore,  a  continuous  deformation  exists  between  p,  and  pj. 
This  proves  the  validity  of  the  Hausdorff  metric  as  a  test  for  path  equivalence.  □ 


3.3  Resolution  Completeness  of  Path  Classifier 

In  this  section,  we  show  that  Alg.  4  is  resolution  complete.  Resolution  complete¬ 
ness  commonly  shows  that  for  a  sufficiently  high  discretization  of  each  dimension 
of  the  search  space,  the  planner  finds  a  path  exactly  when  one  exists  in  the  contin¬ 
uum  space.  We  instead  show  that  for  a  sufficiently  low  dispersion  in  the  infinite¬ 
dimensional  path  space,  the  approximation  given  by  Alg.  4  has  the  same  connectiv¬ 
ity  as  the  continuum  safe,  feasible  path  space. 

Let  T  be  the  continuum  feasible  path  space  and  3^ free  C  T  be  the  set  of  safe, 
feasible  paths.  Using  the  Green-Kelly  algorithm,  we  sample  offline  from  T  a  path 
sequence  T  of  size  N.  At  runtime,  using  Alg.  1,  we  test  members  of  T  in  order  to 
discover  a  set  f  free  C  T  of  safe  paths. 

The  following  lemma  is  based  on  the  work  of  La  Valle,  Branicky,  and  Linde- 
mann  [16],  who  prove  resolution  completeness  of  deterministic  roadmap  (DRM) 
planners,  which  are  PRM  planners  that  draw  samples  from  a  low-dispersion,  deter¬ 
ministic  source.  Since  we  use  a  deterministic  sequence  provided  by  Green-Kelly, 
the  combination  of  Alg.  1  and  4  generates  a  DRM  in  path  space. 

Lemma  3.  For  any  given  configuration  of  obstacles  and  any  path  set  Tat  generated 
by  the  Green-Kelly  algorithm,  there  exists  a  sufficiently  large  N  such  that  any  two 
paths  Pi,Pj  G  ffree  ^re  in  the  same  connected  component  of  J free  if  and  only  if 
Alg.  4  reports  that  pi  ~  pj. 

Proof.  LaValle,  et  al.  [16],  show  that  by  increasing  N,  a  sufficiently  low  dispersion 
can  be  achieved  to  make  a  DRM  complete  in  any  given  C-Space.  By  an  identical 
argument,  given  a  continuum  connected  component  C  C  3' free,  all  sampled  paths 
in  6  n  Vn  are  in  a  single  partition  of  D.  If  q  is  the  radius  of  the  narrowest  corridor 
in  C,  then  for  dispersion  5^  <  q,  our  discrete  approximation  exactly  replicates  the 
connectivity  of  the  continuum  freespace.  □ 

Lemma  4.  Under  the  same  conditions  as  in  Lemma  3,  there  exists  a  sufficiently 
large  N  such  that  for  any  continuum  connected  component  C  C  3^ free,  ^ilg.  1  returns 
a  f  free  such  that  fifree  H  C  7^  0.  That  is,  every  component  in  3^ free  has  a  correspond¬ 
ing  partition  returned  by  Alg.  4. 

Proof.  Let  Br  be  the  largest  open  ball  of  radius  r  in  C.  When  5n  <  r,  Br  must  contain 
some  sample  p  Gfi.  Since  C  is  entirely  collision-free,  p  G  fifree-  Thus,  for  dispersion 
less  than  r,  f  free  contains  a  path  in  C.  □ 


14 


Ross  A.  Knepper,  Siddhartha  S.  Srinivasa,  and  Matthew  T.  Mason 


There  exists  a  sufficiently  large  N  such  that  after  N  samples,  T  has  achieved 
dispersion  5^  <  min(^,r),  where  q  and  r  are  the  dispersion  required  by  Lemmas  3 
and  4,  respectively.  Under  such  conditions,  a  bijection  exists  between  the  connected 
components  of  f  free  and  'J free- 

Theorem  2.  Let  D  =  {Di  | . . .  {‘Dm}  be  a  partition  of  T free  os  defined  by  Alg.  4.  Let 
C  =  {Cl  I . . .  \Gm}  be  a  finite  partition  of  the  continuum  safe,  feasible  path  space  into 
connected  components.  A  bijection  f  :  D  ^  C  exists  such  that  D,-  C  f{'j^i). 

Proof  Lemma  3  establishes  that  /  is  one-to-one,  while  Lemma  4  establishes  that  / 
is  onto.  Therefore,  /  is  bijective.  This  shows  that  by  sampling  at  sufficiently  high 
density,  we  can  achieve  an  arbitrarily  good  approximation  of  the  connectedness  of 
the  continuum  set  of  collision-free  paths  in  any  environment.  □ 

Theorem  3.  A  path  interval  p  may  be  implicitly  tested  safe  if  it  is  between  paths 
Pi  and  pj  such  that  puiPijPj)  d  and  a  small  region  at  the  end  of  pc  has  been 
explicitly  tested. 

Proof.  By  Lemma  2,  the  initial  interval  of  pc  is  safe  because  its  swath  is  covered 
by  the  swaths  of  the  guard  paths.  Since  the  small  interval  at  the  end  of  pc  has  been 
explicitly  tested,  the  whole  of  pc  is  collision-free.  □ 


4  Results 

We  briefly  summarize  some  experimental  results  involving  equivalence  class  detec¬ 
tion  and  implicit  path  collision-testing.  All  tests  were  performed  in  simulation  on 
planning  problems  of  the  type  described  in  [13]. 

Path  classification  imposes  a  computational  overhead  due  to  the  cost  of  searching 
collision-free  paths.  Collision  rate  in  turn  relates  to  the  density  of  obstacles  in  the 
environment.  The  computational  overhead  of  our  classification  implementation  is 
nearly  20%  in  an  empty  environment  but  drops  to  0.3%  in  dense  clutter.  However, 
implicit  collision-testing  more  than  compensates  for  this  overhead. 

Fig.  9  shows  the  effect  of  implicit  path  testing  on  total  paths  tested  in  the  absence 
of  obstacles.  As  the  time  limit  increases,  the  number  of  paths  collision-tested  un¬ 
der  the  traditional  algorithm  increases  linearly  at  a  rate  of  8,300  paths  per  second. 
With  implicit  testing,  the  initial  test  rate  over  small  time  limits  (thus  small  path  set 
sizes)  is  over  22,500  paths  per  second.  The  marginal  rate  declines  over  time  due 
to  the  aforementioned  overhead,  but  implicit  path  testing  still  maintains  its  speed 
advantage  until  the  entire  2,401 -member  path  set  is  collision-tested. 

Fig.  10  presents  implicit  collision-testing  performance  in  the  presence  of  clutter. 
We  compare  the  implicit  collision-tester  in  Alg.  5  to  traditional  explicit  collision¬ 
testing.  When  fixing  the  replan  rate  at  10  Hz,  implicit  path  evaluation  maintains  an 
advantage,  despite  the  overhead,  across  all  navigable  obstacle  densities. 
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Replan  CutoK  Time  (sec] 


Fig.  9  Paths  tested  per  time-limited  replan  step 
in  an  obstacle-free  environment.  Path  testing 
performance  improves  by  up  to  3x  with  the  al¬ 
gorithms  we  present  here.  Note  that  an  artificial 
ceiling  curtails  performance  at  the  high  end  due 
to  a  maximum  path  set  of  size  2,401. 


5  Discussion  and  Future  Work 
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Fig.  10  Paths  tested  per  0.1  second  time  step 
at  varying  obstacle  densities.  Implicit  collision¬ 
testing  allows  significantly  more  paths  to  be 
tested  per  unit  time.  Even  in  extremely  dense 
clutter,  implicit  path  testing  considers  an  extra 
six  paths  on  average. 


In  this  paper,  we  propose  an  equivalence  relation  on  local  paths  based  on  the  fol¬ 
lowing  constraints;  fixed  start  position  and  heading,  fixed  length,  and  bounded  cur¬ 
vature.  We  describe  an  algorithm  for  easily  classifying  paths  using  the  Hausdorff 
distance  between  them.  Path  classification  is  a  tool  that  permits  collective  reasoning 
about  paths,  leading  to  more  efficient  collision-testing. 

There  are  many  other  applications  for  path  equivalence.  One  example  uses  path 
class  knowledge  in  obstacle  avoidance  to  improve  visibility  and  safety  around  ob¬ 
stacles.  Another  avenue  of  future  work  involves  generalizing  path  equivalence  to 
higher  dimensions.  For  instance,  an  implicit  path  test  for  a  robot  floating  in  3D  re¬ 
quires  three  neighboring  paths,  while  a  manipulator  arm  needs  only  two. 
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